package org.noote.libs.roby.comm;

public class BluetoothCommandProtocol {

	//////////////////////////////////
	///
	/// Only for Roby Arduino Sketch 0.9 !
	///
	/////////////////////////////////
	static final String CMD_START_TRAME = "#";
	static final String CMD_END_TRAME = "$";
	protected static String _bounding(String sCmd)
	{
		return CMD_START_TRAME+sCmd+CMD_END_TRAME;
	}
	
	
	/////////////////////////////////
	// Version
	static final String CMD_VERSION = "v";
	public static String sendAskVersion()
	{
		return _bounding(CMD_VERSION);
	}

	/////////////////////////////////
	// Setting
	static final String CMD_SETTING = "g";
	public static String sendAskSetting()
	{
		return _bounding(CMD_SETTING+"a-");
	}
	public static String sendAskSettingBody()
	{
		return _bounding(CMD_SETTING+"ab");
	}
	public static String sendAskSettingTurret()
	{
		return _bounding(CMD_SETTING+"at");
	}
	public static String sendAskSettingRay()
	{
		return _bounding(CMD_SETTING+"ar");
	}
	public static String sendSetSettingRayCount(int iCount)
	{
		return _bounding(CMD_SETTING+"src"+Integer.toString(iCount));
	}
	public static String sendSetSettingRayDelay(int iDelay)
	{
		return _bounding(CMD_SETTING+"srd"+Integer.toString(iDelay));
	}
	public static String sendSetSettingRaySensors(int sSensors)
	{
		return _bounding(CMD_SETTING+"srs"+Integer.toString(sSensors));
	}
	public static String sendSetSettingRayPrecision(float fPrecision)
	{
		int iP = (int)((fPrecision>1.0f?1:fPrecision<0.0f?0:fPrecision)*100.0f);
		return _bounding(CMD_SETTING+"srp"+Integer.toString(iP));
	}
	/* TODO
	public static String sendSetSettingRay(int iCount, int iDelay, int sSensors, float fPrecision)
	{
		return _bounding(CMD_SETTING+"sra"+Integer.toString(iCount));
	}
	 */

	/////////////////////////////////
	// Status
	static final String CMD_STATUS = "s";
	public static String sendAskStatus()
	{
		return _bounding(CMD_STATUS+"a-");
	}
	public static String sendAskStatusSensors()
	{
		return _bounding(CMD_STATUS+"as");
	}
	public static String sendAskStatusTurret()
	{
		return _bounding(CMD_STATUS+"at");
	}
	public static String sendAskStatusBody()
	{
		return _bounding(CMD_STATUS+"ab");
	}
	public static String sendSetSendStatus(boolean bSet)
	{
		return _bounding(CMD_STATUS+"ss"+(bSet?"1":"0"));
	}

	/////////////////////////////////
	// Reset
	static final String CMD_RESET = "r";
	public static String sendReset()
	{
		return _bounding(CMD_RESET+"-");
	}
	public static String sendResetBody()
	{
		return _bounding(CMD_RESET+"b");
	}
	public static String sendResetTurret()
	{
		return _bounding(CMD_RESET+"t");
	}

	/////////////////////////////////
	// Body
	static final String CMD_BODY = "b";
	public static String sendBodyFront(int iSpeed)
	{
		String sSpeed = (iSpeed<0)?"0":(iSpeed>100)?"100":Integer.toString(iSpeed);
		if(iSpeed>0) sSpeed = "+"+sSpeed;
		return _bounding(CMD_BODY+"m"+"11"+sSpeed+sSpeed);
	}
	public static String sendBodyBack(int iSpeed)
	{
		String sSpeed = (iSpeed<0)?"0":(iSpeed>100)?"100":Integer.toString(iSpeed);
		if(iSpeed>0) sSpeed = "+"+sSpeed;
		return _bounding(CMD_BODY+"m"+"00"+sSpeed+sSpeed);
	}
	public static String sendBodyLeft(int iSpeed)
	{
		String sSpeed = (iSpeed<0)?"0":(iSpeed>100)?"100":Integer.toString(iSpeed);
		if(iSpeed>0) sSpeed = "+"+sSpeed;
		return _bounding(CMD_BODY+"m"+"10"+sSpeed+sSpeed);
	}
	public static String sendBodyRight(int iSpeed)
	{
		String sSpeed = (iSpeed<0)?"0":(iSpeed>100)?"100":Integer.toString(iSpeed);
		if(iSpeed>0) sSpeed = "+"+sSpeed;
		return _bounding(CMD_BODY+"m"+"01"+sSpeed+sSpeed);
	}
	public static String sendBodyMoveToTarget(int iSpeed, boolean bFront, int iLen)
	{
		String sSpeed = (iSpeed<0)?"0":(iSpeed>100)?"100":Integer.toString(iSpeed);
		String sFront = (bFront)?"1":"0";
		String sLen = (iLen<0)?"0":(iLen>100)?"100":Integer.toString(iLen);
		if(iSpeed>0) sSpeed = "+"+sSpeed;
		if(iLen>0) sLen = "+"+sLen;
		return _bounding(CMD_BODY+"tm"+sFront+sSpeed+sLen);
	}
	public static String sendBodyRotateToTarget(int iSpeed, int iBearing)
	{
		int iBear = iBearing%360;
		if(iBear<0) iBear+=360;
		
		String sSpeed = (iSpeed<0)?"0":(iSpeed>100)?"100":Integer.toString(iSpeed);
		String sBear = Integer.toString(iBear);
		if(iSpeed>0) sSpeed = "+"+sSpeed;
		if(iBearing>0) sBear = "+"+sBear;
		return _bounding(CMD_BODY+"tr"+sSpeed+sBear);
	}

	/////////////////////////////////
	// Turret
	static final String CMD_TURRET = "t";
	public static String sendTurretSetPosition(short low, short high)
	{
		String sLow = Integer.toString(low);
		String sHigh = Integer.toString(high);
		if(low>0) sLow = "+"+sLow;
		if(high>0) sHigh = "+"+sHigh;
		return _bounding(CMD_TURRET+"p"+sLow+sHigh);
	}
	public static String sendTurretScanPosition(short low, short high)
	{
		String sLow = Integer.toString(low);
		String sHigh = Integer.toString(high);
		if(low>0) sLow = "+"+sLow;
		if(high>0) sHigh = "+"+sHigh;
		return _bounding(CMD_TURRET+"sp"+sLow+sHigh);
	}
	public static String sendTurretScanLine(short low_beg, short low_end, short low_step, short high)
	{
		String sLowBeg = Integer.toString(low_beg);
		String sLowEnd = Integer.toString(low_end);
		String sLowStep = Integer.toString(low_step);
		String sHigh = Integer.toString(high);
		if(low_beg>0) sLowBeg = "+"+sLowBeg;
		if(low_end>0) sLowEnd = "+"+sLowEnd;
		if(low_step>0) sLowStep = "+"+sLowStep;
		if(high>0) sHigh = "+"+sHigh;
		return _bounding(CMD_TURRET+"sl"+sLowBeg+sLowEnd+sLowStep+sHigh);
	}
	public static String sendTurretScanZone(short low_beg, short low_end, short low_step, short high_beg, short high_end, short high_step)
	{
		String sLowBeg = Integer.toString(low_beg);
		String sLowEnd = Integer.toString(low_end);
		String sLowStep = Integer.toString(low_step);
		String sHighBeg = Integer.toString(high_beg);
		String sHighEnd = Integer.toString(high_end);
		String sHighStep = Integer.toString(high_step);
		if(low_beg>0) sLowBeg = "+"+sLowBeg;
		if(low_end>0) sLowEnd = "+"+sLowEnd;
		if(low_step>0) sLowStep = "+"+sLowStep;
		if(high_beg>0) sHighBeg = "+"+sHighBeg;
		if(high_end>0) sHighEnd = "+"+sHighEnd;
		if(high_step>0) sHighStep = "+"+sHighStep;
		return _bounding(CMD_TURRET+"sz"+sLowBeg+sLowEnd+sLowStep+sHighBeg+sHighEnd+sHighStep);
	}
}
